/*
    ModbusIP.cpp - Source for Modbus IP Library
    Copyright (C) 2015 André Sarmento Barbosa
*/
#include "ModbusIP.h"

WiFiServer server(MODBUSIP_PORT);

ModbusIP::ModbusIP()
{
}

void ModbusIP::config()
{
    server.begin();
    Serial.printf("config\r\n");
}


int ModbusIP::prcess(uint8_t ch)
{
    switch (state)
    {
    case 0:
        _MBAP[mb_cnt] = ch;
        mb_cnt++;
        if (mb_cnt == 1)
        {
            makr1 = millis();
        }
        else if (mb_cnt >= 7)
        {
            mbap_len = _MBAP[4] << 8 | _MBAP[5];
            // uint16_t mbap_uid;
            // mbap_uid = _MBAP[6];
            // Serial.printf("protocol id:0X%02X,0X%02X\n", _MBAP[2], _MBAP[3]);
            // Serial.printf("len :%d\n", mbap_len);
            // Serial.printf("uid :%d\n", mbap_uid);
            state = 1;
            _frame = (byte *)malloc(mbap_len);
            frame_cnt = 0;
        }
        break;
    case 1:
        _frame[frame_cnt++] = ch;
        if (frame_cnt >= mbap_len - 1)
        {
            cost = millis() - makr1;
            log_print_buf(_MBAP, mb_cnt);
            log_print_buf(_frame, frame_cnt);
            state = 0;
            mb_cnt = 0;
            this->receivePDU(_frame);
            if (_reply != MB_REPLY_OFF)
            {
                // MBAP
                _MBAP[4] = (_len + 1) >> 8; //_len+1 for last byte from MBAP
                _MBAP[5] = (_len + 1) & 0x00FF;

                int send_len = (unsigned int)_len + 7;

                return send_len;
                // Serial.printf("sendbuf\n");
                // log_print_buf(sbuf, send_len);
                // Serial.printf("\n");
            }
        }
        break;
    default:
        state = 0;
        break;
    }
    return -1;
}
void ModbusIP::task()
{
    WiFiClient client = server.available();

    int raw_len = 0;
    uint32_t starttime = 0;

    if (client)
    {
        client.setTimeout(10);
        while (1)
        {
            starttime = millis();
            if (client.available())
            {
                int send_len = prcess(client.read());
                if (send_len >= 0)
                {
                    uint8_t send_buf[send_len];

                    for (int i = 0; i < 7; i++)
                        send_buf[i] = _MBAP[i];
                    for (int i = 0; i < _len; i++)
                        send_buf[i + 7] = _frame[i];

                    client.write(send_buf, send_len);

                    free(_frame);
                    _len = 0;
                    Serial.printf("cost :%d heap:%0.3f\n", cost, ESP.getFreeHeap() / 1024.0);
                }
                // Serial.printf("cost1 = %d, len = %d\n", millis() - starttime, client.available());
            }
            yield();
        }
    }
}